﻿#include "algo_interpolate.h"
#include <Dense>
#include <Sparse>

Handle(CAGD::GeomBSplineCurve) CAGD::Interpolate::BSplineCurve3(const std::vector<BPnt> &points)
{
    // 只能是 3 次
    int p = 3;
    int K = points.size() - 1;

    // 生成均匀节点向量（这里可以改为通过外部传入参数值作为节点向量）
    std::vector<double> tau;
    for (int i = 0; i < p + 1; i++)
        tau.push_back(0);
    for (int i = 0; i < K; i++)
        tau.push_back((i + 1) * 1.0 / K);
    for (int i = 0; i < p; i++)
        tau.push_back(1);

    // 系数间隔
    double h = 1.0 / K;

    // 填系数矩阵
    std::vector<Eigen::Triplet<double>> trip;
    for (int i = 1; i < K; i++)
    {
        auto res = Spline::Basis(i + p, tau[i + p], p, tau);
        trip.push_back({i, i - 1, res[0]});
        trip.push_back({i, i, res[1]});
        trip.push_back({i, i + 1, res[2]});
    }

    // 首尾系数（自然边界条件）
    trip.push_back({0, 0, -3 * h});
    trip.push_back({0, 1, h});
    trip.push_back({K, K - 1, h});
    trip.push_back({K, K, -3 * h});

    Eigen::SparseMatrix<double> M(K + 1, K + 1);
    M.setFromTriplets(trip.begin(), trip.end());

    // 填充 Q
    Eigen::MatrixXd Q(K + 1, 3);
    // Q.setZero();
    for (int i = 1; i < K; i++)
    {
        Q(i, 0) = points[i].X();
        Q(i, 1) = points[i].Y();
        Q(i, 2) = points[i].Z();
    }

    // 首尾
    BVec P0 = points[0] * -2 * h;
    BVec PK2 = points[K] * -2 * h;
    Q(0, 0) = P0.X();
    Q(0, 1) = P0.Y();
    Q(0, 2) = P0.Z();
    Q(K, 0) = PK2.X();
    Q(K, 1) = PK2.Y();
    Q(K, 2) = PK2.Z();

    // 求解
    M.makeCompressed();
    Eigen::SparseLU<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>> solver;

    // 计算置换向量
    solver.analyzePattern(M);
    solver.factorize(M);

    Eigen::MatrixXd P = solver.solve(Q);
    std::vector<BPnt> cpts;
    cpts.push_back(points[0]);
    for (int i = 0; i < K + 1; i++)
        cpts.push_back(BPnt(P(i, 0), P(i, 1), P(i, 2)));
    cpts.push_back(points[K]);

    return Handle(GeomBSplineCurve)(new GeomBSplineCurve(tau, cpts));
}

Handle(CAGD::GeomBSplineSurface) CAGD::Interpolate::BSplineSurface3x3(const std::vector<std::vector<BPnt>> &points)
{
    int nu = points.size();
    int nv = points.front().size();

    std::vector<double> uknots;
    std::vector<double> vknots;

    // 插值后翻转方向，便于在另一个方向插值
    // 注意每次插值以后都会多 2 个控制点
    std::vector<std::vector<CAGD::BPnt>> tar2(nv + 2, std::vector<CAGD::BPnt>(nu));
    for (int i = 0; i < nu; i++)
    {
        Handle(GeomBSplineCurve) spline = BSplineCurve3(points[i]);
        auto &cpt = spline->Poles();
        for (int j = 0; j < nv + 2; j++)
            tar2[j][i] = cpt[j];

        vknots = spline->Knots();
    }

    std::vector<std::vector<CAGD::BPnt>> tar3(nu + 2, std::vector<CAGD::BPnt>(nv + 2));
    for (int i = 0; i < nv + 2; i++)
    {
        Handle(GeomBSplineCurve) spline = BSplineCurve3(tar2[i]);
        auto &cpt = spline->Poles();
        for (int j = 0; j < nu + 2; j++)
            tar3[j][i] = cpt[j];

        uknots = spline->Knots();
    }

    return Handle(GeomBSplineSurface)(new GeomBSplineSurface(uknots, vknots, tar3));
}
